#include "hand_eye_calibrator.h"


void HEcalibrator::readFileJson(int count,vector<string> filepath_json )
{
    Json::Reader reader;
    Json::Value root;

    //从文件中读取，保证当前文件有demo.json文件
    ifstream in(filepath_json[count]);

    if (!in.is_open())
    {
        cout << "Error opening file\n";
        return;
    }

    if (reader.parse(in, root))
    {

        orientation_x = root["orientation"]["x"].asDouble();
        orientation_y = root["orientation"]["y"].asDouble();
        orientation_z = root["orientation"]["z"].asDouble();
        orientation_w = root["orientation"]["w"].asDouble();
        position_x = root["position"]["x"].asDouble();
        position_y = root["position"]["y"].asDouble();
        position_z = root["position"]["z"].asDouble();
    }
    else
    {
        cout << "parse error\n"
             << endl;
    }

    in.close();
}

void HEcalibrator::getFileName(char * dirPath,vector<string>&files,string suffix)
{
    string p;
    DIR *dir=opendir(dirPath);
    if(dir==NULL)
    {
            printf("%s\n",strerror(errno));
            return;
    }       
    chdir(dirPath);//进入到当前读取目录
    struct dirent *ent;
    while((ent=readdir(dir))!=NULL)
    {
        if( strcmp( ent->d_name , "." ) == 0 ||strcmp( ent->d_name , "..") == 0)
        continue;
        string sFilename(ent->d_name);
        string suffixStr = sFilename.substr(sFilename.find_last_of('.') + 1);//获取文件后缀
        if (suffixStr.compare(suffix) == 0) 
        {//根据后缀筛选文件
            files.push_back(p.assign(dirPath).append("/").append(ent->d_name));
        }

    }       
    closedir(dir);
    sort(files.begin(),files.end());
    // for(int i=0;i<files.size();i++)
    // {
    //     cout<<files[i]<<endl;
    // }
}


void HEcalibrator::calibrateRobotWorldHandEye(InputArrayOfArrays R_world2cam, InputArrayOfArrays t_world2cam,
                                              InputArrayOfArrays R_base2gripper, InputArrayOfArrays t_base2gripper,
                                              OutputArray R_base2world, OutputArray t_base2world,
                                              OutputArray R_gripper2cam, OutputArray t_gripper2cam,
                                              RobotWorldHandEyeCalibrationMethod method)
{
    CV_Assert(R_base2gripper.isMatVector() && t_base2gripper.isMatVector() &&
              R_world2cam.isMatVector() && t_world2cam.isMatVector());

    std::vector<Mat> R_base2gripper_tmp, t_base2gripper_tmp;
    R_base2gripper.getMatVector(R_base2gripper_tmp);
    t_base2gripper.getMatVector(t_base2gripper_tmp);

    std::vector<Mat> R_world2cam_tmp, t_world2cam_tmp;
    R_world2cam.getMatVector(R_world2cam_tmp);
    t_world2cam.getMatVector(t_world2cam_tmp);

    CV_Assert(R_base2gripper_tmp.size() == t_base2gripper_tmp.size() &&
              R_world2cam_tmp.size() == t_world2cam_tmp.size() &&
              R_base2gripper_tmp.size() == R_world2cam_tmp.size());
    CV_Check(R_base2gripper_tmp.size(), R_base2gripper_tmp.size() >= 3, "At least 3 measurements are needed");

    // Convert to double
    std::vector<Mat_<double>> R_base2gripper_, t_base2gripper_;
    std::vector<Mat_<double>> R_world2cam_, t_world2cam_;

    R_base2gripper_.reserve(R_base2gripper_tmp.size());
    t_base2gripper_.reserve(R_base2gripper_tmp.size());
    R_world2cam_.reserve(R_world2cam_tmp.size());
    t_world2cam_.reserve(R_base2gripper_tmp.size());

    // Convert to rotation matrix if needed
    for (size_t i = 0; i < R_base2gripper_tmp.size(); i++)
    {
        {
            Mat rot = R_base2gripper_tmp[i];
            Mat R(3, 3, CV_64FC1);
            if (rot.size() == Size(3, 3))
            {
                rot.convertTo(R, CV_64F);
                R_base2gripper_.push_back(R);
            }
            else
            {
                Rodrigues(rot, R);
                R_base2gripper_.push_back(R);
            }
            Mat tvec = t_base2gripper_tmp[i];
            Mat t;
            tvec.convertTo(t, CV_64F);
            t_base2gripper_.push_back(t);
        }
        {
            Mat rot = R_world2cam_tmp[i];
            Mat R(3, 3, CV_64FC1);
            if (rot.size() == Size(3, 3))
            {
                rot.convertTo(R, CV_64F);
                R_world2cam_.push_back(R);
            }
            else
            {
                Rodrigues(rot, R);
                R_world2cam_.push_back(R);
            }
            Mat tvec = t_world2cam_tmp[i];
            Mat t;
            tvec.convertTo(t, CV_64F);
            t_world2cam_.push_back(t);
        }
    }

    CV_Assert(R_world2cam_.size() == t_world2cam_.size() &&
              R_base2gripper_.size() == t_base2gripper_.size() &&
              R_world2cam_.size() == R_base2gripper_.size());

    Matx33d wRb, cRg;
    Matx31d wtb, ctg;
    switch (method)
    {
    case CALIB_ROBOT_WORLD_HAND_EYE_SHAH:
        calibrateRobotWorldHandEyeShah(R_world2cam_, t_world2cam_, R_base2gripper_, t_base2gripper_, wRb, wtb, cRg, ctg);
        break;

    case CALIB_ROBOT_WORLD_HAND_EYE_LI:
        calibrateRobotWorldHandEyeLi(R_world2cam_, t_world2cam_, R_base2gripper_, t_base2gripper_, wRb, wtb, cRg, ctg);
        break;
    }

    Mat(wRb).copyTo(R_base2world);
    Mat(wtb).copyTo(t_base2world);

    Mat(cRg).copyTo(R_gripper2cam);
    Mat(ctg).copyTo(t_gripper2cam);
}


void HEcalibrator::calibrateRobotWorldHandEyeShah(const std::vector<Mat_<double>> &cRw, const std::vector<Mat_<double>> &ctw,
                                                  const std::vector<Mat_<double>> &gRb, const std::vector<Mat_<double>> &gtb,
                                                  Matx33d &wRb, Matx31d &wtb, Matx33d &cRg, Matx31d &ctg)
{
    Mat_<double> T = Mat_<double>::zeros(9, 9);
    for (size_t i = 0; i < cRw.size(); i++)
    {
        T += kron(gRb[i], cRw[i]);
    }

    Mat_<double> w, u, vt;
    SVDecomp(T, w, u, vt);

    Mat_<double> RX(3, 3), RZ(3, 3);
    for (int i = 0; i < 3; i++)
    {
        for (int j = 0; j < 3; j++)
        {
            RX(j, i) = vt(0, i * 3 + j);
            RZ(j, i) = u(i * 3 + j, 0);
        }
    }

    wRb = normalizeRotation(RX);
    cRg = normalizeRotation(RZ);
    Mat_<double> Z = Mat(cRg.t()).reshape(1, 9);

    const int n = static_cast<int>(cRw.size());
    Mat_<double> A = Mat_<double>::zeros(3 * n, 6);
    Mat_<double> b = Mat_<double>::zeros(3 * n, 1);
    Mat_<double> I3 = Mat_<double>::eye(3, 3);

    for (int i = 0; i < n; i++)
    {
        Mat cRw_ = -cRw[i];
        cRw_.copyTo(A(Range(i * 3, (i + 1) * 3), Range(0, 3)));
        I3.copyTo(A(Range(i * 3, (i + 1) * 3), Range(3, 6)));

        Mat ctw_ = ctw[i] - kron(gtb[i].t(), I3) * Z;
        ctw_.copyTo(b(Range(i * 3, (i + 1) * 3), Range::all()));
    }

    Mat_<double> t;
    solve(A, b, t, DECOMP_SVD);

    for (int i = 0; i < 3; i++)
    {
        wtb(i) = t(i);
        ctg(i) = t(i + 3);
    }
}


//Reference:
//A. Li, L. Wang, and D. Wu, "Simultaneous robot-world and hand-eye calibration using dual-quaternions and kronecker product"
//International Journal of Physical Sciences, vol. 5, pp. 1530–1536, 2010.
//Matlab code: http://math.loyola.edu/~mili/Calibration/
void HEcalibrator::calibrateRobotWorldHandEyeLi(const std::vector<Mat_<double>> &cRw, const std::vector<Mat_<double>> &ctw,
                                                const std::vector<Mat_<double>> &gRb, const std::vector<Mat_<double>> &gtb,
                                                Matx33d &wRb, Matx31d &wtb, Matx33d &cRg, Matx31d &ctg)
{
    const int n = static_cast<int>(cRw.size());
    Mat_<double> A = Mat_<double>::zeros(12 * n, 24);
    Mat_<double> b = Mat_<double>::zeros(12 * n, 1);
    Mat_<double> I3 = Mat_<double>::eye(3, 3);

    for (int i = 0; i < n; i++)
    {
        //Eq 19
        kron(cRw[i], I3).copyTo(A(Range(i * 12, i * 12 + 9), Range(0, 9)));
        kron(-I3, gRb[i].t()).copyTo(A(Range(i * 12, i * 12 + 9), Range(9, 18)));

        kron(I3, gtb[i].t()).copyTo(A(Range(i * 12 + 9, (i + 1) * 12), Range(9, 18)));
        Mat cRw_ = -cRw[i];
        cRw_.copyTo(A(Range(i * 12 + 9, (i + 1) * 12), Range(18, 21)));
        I3.copyTo(A(Range(i * 12 + 9, (i + 1) * 12), Range(21, 24)));

        ctw[i].copyTo(b(Range(i * 12 + 9, i * 12 + 12), Range::all()));
    }

    Mat_<double> x;
    solve(A, b, x, DECOMP_SVD);

    Mat_<double> RX = x(Range(0, 9), Range::all()).reshape(3, 3);
    wRb = normalizeRotation(RX);
    x(Range(18, 21), Range::all()).copyTo(wtb);

    Mat_<double> RZ = x(Range(9, 18), Range::all()).reshape(3, 3);
    cRg = normalizeRotation(RZ);
    x(Range(21, 24), Range::all()).copyTo(ctg);
}


Mat HEcalibrator::kron(const Mat &A, const Mat &B)
{
    CV_Assert(A.channels() == 1 && B.channels() == 1);

    Mat1d Ad, Bd;
    A.convertTo(Ad, CV_64F);
    B.convertTo(Bd, CV_64F);

    Mat1d Kd(Ad.rows * Bd.rows, Ad.cols * Bd.cols, 0.0);
    for (int ra = 0; ra < Ad.rows; ra++)
    {
        for (int ca = 0; ca < Ad.cols; ca++)
        {
            Kd(Range(ra * Bd.rows, (ra + 1) * Bd.rows), Range(ca * Bd.cols, (ca + 1) * Bd.cols)) = Bd.mul(Ad(ra, ca));
        }
    }

    Mat K;
    Kd.convertTo(K, A.type());
    return K;
}


Mat_<double> HEcalibrator::normalizeRotation(const Mat_<double> &R_)
{
    // Make R unit determinant
    Mat_<double> R = R_.clone();
    double det = determinant(R);
    if (std::fabs(det) < FLT_EPSILON)
    {
        CV_Error(Error::StsNoConv, "Rotation normalization issue: determinant(R) is null");
    }
    R = std::cbrt(std::copysign(1, det) / std::fabs(det)) * R;

    // Make R orthogonal
    Mat w, u, vt;
    SVDecomp(R, w, u, vt);
    R = u * vt;

    // Handle reflection case
    if (determinant(R) < 0)
    {
        Matx33d diag(1.0, 0.0, 0.0,
                     0.0, 1.0, 0.0,
                     0.0, 0.0, -1.0);
        R = u * diag * vt;
    }

    return R;
}

void HEcalibrator::calibrateSensors()
{
    ROS_INFO("READY TO READ IMAGE");
    // 读取每一幅图像，从中提取出角点，然后对角点进行亚像素精确化
    Size image_size;                                             /* 图像的尺寸 */
    Size board_size = Size(chessboard_width, chessboard_height); /* 标定板上每行、列的角点数 */
    vector<Point2f> image_points_buf;                            /* 缓存每幅图像上检测到的角点 */
    vector<vector<Point2f>> image_points_seq;                    /* 保存检测到的所有角点 */
    vector<string> filepath_image;
    vector<string> filepath_json;
    char data_path_temp[100];
    strcpy(data_path_temp,data_path.c_str());
    // cout<<data_path_temp<<endl;
    // cout<<data_path<<endl;
    getFileName(data_path_temp,filepath_image,"jpg");
    getFileName(data_path_temp,filepath_json,"json");
    int image_count=filepath_image.size();
    
    for (int num = 0; num < image_count; num++)
    {
        Mat imageInput = imread(filepath_image[num]);

        // 读入第一张图片时获取图片大小
        if (num == 0)
        {
            image_size.width = imageInput.cols;
            image_size.height = imageInput.rows;
        }

        /* 提取角点 */
        if (0 == findChessboardCorners(imageInput, board_size, image_points_buf))
        {
            cout << "can not find chessboard corners!\n"; // 找不到角点
            exit(1);
        }
        else
        {
            //cout<<"    "<<to_string(num)<<"    "<<image_points_buf<<endl;   // order : left to right ,then jump to next row
            Mat view_gray;
            cvtColor(imageInput, view_gray, COLOR_RGB2GRAY); // 转灰度图

            /* 亚像素精确化 */
            // image_points_buf 初始的角点坐标向量，同时作为亚像素坐标位置的输出
            // Size(5,5) 搜索窗口大小
            // （-1，-1）表示没有死区
            // TermCriteria 角点的迭代过程的终止条件, 可以为迭代次数和角点精度两者的组合
            cornerSubPix(view_gray, image_points_buf, Size(5, 5), Size(-1, -1), TermCriteria(TermCriteria::EPS + TermCriteria::COUNT, 30, 0.1));

            image_points_seq.push_back(image_points_buf); // 保存亚像素角点

            /* 在图像上显示角点位置 */
            //    drawChessboardCorners(view_gray, board_size, image_points_buf, false); // 用于在图片中标记角点
            //    line(view_gray,image_points_buf[0], image_points_buf[6],Scalar(255,0,0));
            //    line(view_gray,image_points_buf[6], image_points_buf[7],Scalar(255,0,0));
            //    imshow("Camera Calibration", view_gray);       // 显示图片

            //    waitKey(1000); //暂停1S
        }
    }
    int CornerNum = board_size.width * board_size.height; // 每张图片上总的角点数

    //-------------以下是摄像机标定------------------

    /*棋盘三维信息*/
    Size square_size = Size(square_dimension, square_dimension); /* 实际测量得到的标定板上每个棋盘格的大小 */
    vector<vector<Point3f>> object_points;                       /* 保存标定板上角点的三维坐标 */

    /*内外参数*/
    Mat cameraMatrix = Mat(3, 3, CV_32FC1, Scalar::all(0)); /* 摄像机内参数矩阵 */
    vector<int> point_counts;                               // 每幅图像中角点的数量
    Mat distCoeffs = Mat(1, 5, CV_32FC1, Scalar::all(0));   /* 摄像机的5个畸变系数：k1,k2,p1,p2,k3 */
    vector<Mat> tvecsMat;                                   /* 每幅图像的旋转向量 */
    vector<Mat> rvecsMat;                                   /* 每幅图像的平移向量 */

    vector<Mat> rotation_mats(image_count); /* 每幅图像的旋转矩阵 */
    vector<Mat> tvecsMats(image_count);     /* 每幅图像的平移向量 */

    vector<Mat> base2gripper_R(image_count);
    vector<Mat> base2gripper_T(image_count);
    /* 初始化标定板上角点的三维坐标 */
    int i, j, t;
    for (t = 0; t < image_count; t++)
    {
        vector<Point3f> tempPointSet;
        for (i = 0; i < board_size.height; i++)
        {
            for (j = 0; j < board_size.width; j++)
            {
                Point3f realPoint;

                /* 假设标定板放在世界坐标系中z=0的平面上 */
                realPoint.x = j * square_size.width;
                realPoint.y = i * square_size.height;
                realPoint.z = 0;
                tempPointSet.push_back(realPoint);
            }
        }
        object_points.push_back(tempPointSet);
    }

    /* 初始化每幅图像中的角点数量，假定每幅图像中都可以看到完整的标定板 */
    for (i = 0; i < image_count; i++)
    {
        point_counts.push_back(board_size.width * board_size.height);
    }

    /* 开始标定 */
    // object_points 世界坐标系中的角点的三维坐标
    // image_points_seq 每一个内角点对应的图像坐标点
    // image_size 图像的像素尺寸大小
    // cameraMatrix 输出，内参矩阵
    // distCoeffs 输出，畸变系数
    // rvecsMat 输出，旋转向量
    // tvecsMat 输出，位移向量
    // 0 标定时所采用的算法

    calibrateCamera(object_points, image_points_seq, image_size, cameraMatrix, distCoeffs, rvecsMat, tvecsMat, 0);

    //     ------------------------标定完成------------------------------------

    //     // -------------------对标定结果进行评价------------------------------

    //     double total_err = 0.0;         /* 所有图像的平均误差的总和 */
    //     double err = 0.0;               /* 每幅图像的平均误差 */
    //     vector<Point2f> image_points2;  /* 保存重新计算得到的投影点 */

    //     for (i=0;i<image_count;i++)
    //     {
    //         vector<Point3f> tempPointSet = object_points[i];

    //         /* 通过得到的摄像机内外参数，对空间的三维点进行重新投影计算，得到新的投影点 */
    //         projectPoints(tempPointSet, rvecsMat[i], tvecsMat[i], cameraMatrix, distCoeffs, image_points2);

    //         /* 计算新的投影点和旧的投影点之间的误差*/
    //         vector<Point2f> tempImagePoint = image_points_seq[i];
    //         Mat tempImagePointMat = Mat(1, tempImagePoint.size(), CV_32FC2);
    //         Mat image_points2Mat = Mat(1, image_points2.size(), CV_32FC2);

    //         for (int j = 0 ; j < tempImagePoint.size(); j++)
    //         {
    //             image_points2Mat.at<Vec2f>(0,j) = Vec2f(image_points2[j].x, image_points2[j].y);
    //             tempImagePointMat.at<Vec2f>(0,j) = Vec2f(tempImagePoint[j].x, tempImagePoint[j].y);
    //         }
    //         err = norm(image_points2Mat, tempImagePointMat, NORM_L2);
    //         total_err += err/= point_counts[i];
    //     }

    // //     // //-------------------------评价完成---------------------------------------------

    //-----------------------保存定标结果-------------------------------------------
    ROS_INFO("hand-eye clibration...");
    Mat rotation_matrix = Mat(3, 3, CV_32FC1, Scalar::all(0)); /* 暂存每幅图像的旋转矩阵 */
    Mat tvecs_matrix = Mat(3, 1, CV_32FC1, Scalar::all(0));    /* 暂存每幅图像的旋转矩阵 */
    Mat forinv = Mat(4, 4, CV_64FC1, Scalar::all(0));

    for (int i = 0; i < image_count; i++)
    {
        /* 将旋转向量转换为相对应的旋转矩阵 */
        Rodrigues(rvecsMat[i], rotation_matrix);
        tvecs_matrix = tvecsMat[i] / 1000; // mm to m

        for (int r = 0; r < 3; r++)
        {
            forinv.at<double>(r, 3) = tvecs_matrix.at<double>(r);
            for (int c = 0; c < 3; c++)
            {
                forinv.at<double>(r, c) = rotation_matrix.at<double>(r, c);
            }
        }

        forinv.at<double>(3, 3) = 1;
        forinv = forinv.inv();

        rotation_matrix = forinv(cv::Rect(0, 0, 3, 3)); //从forinv中提取旋转矩阵R
        tvecs_matrix = forinv(cv::Rect(3, 0, 1, 3));    //从forinv中提取平移向量T
        rotation_mats[i] = rotation_matrix.clone();     //旋转阵
        tvecsMats[i] = tvecs_matrix.clone();            //平移阵
    }

    /*      相机与标定板参数已经就绪      */

    /*      准备base2camera_R,base2camera_T     */

    Mat position_mat = Mat(3, 1, CV_64FC1, Scalar::all(0));
    ;
    Eigen::Matrix<double, 3, 3> r_mat;
    for (int i = 0; i < image_count; i++)
    {
        readFileJson(i,filepath_json);
        // 注意wxyz顺序
        Quaterniond q(orientation_w, orientation_x, orientation_y, orientation_z);
        Quaterniond qu = q.normalized();
        //cout<<"rotation matrix:\n"<<qu.matrix()<<endl;
        r_mat = qu.matrix();
        for (int r = 0; r < 3; r++)
        {
            for (int c = 0; c < 3; c++)
            {
                // rotation_matrix.at<double>(c,r) = r_mat(r,c);
                rotation_matrix.at<double>(r, c) = r_mat(r, c);
                // cout <<rotation_matrix.at<double>(r,c)<<endl;
            }
        }

        position_mat.at<double>(0) = position_x;
        position_mat.at<double>(1) = position_y;
        position_mat.at<double>(2) = position_z - 0.5255; //

        for (int r = 0; r < 3; r++)
        {
            forinv.at<double>(r, 3) = position_mat.at<double>(r);
            for (int c = 0; c < 3; c++)
            {
                forinv.at<double>(r, c) = rotation_matrix.at<double>(r, c);
            }
        }

        forinv.at<double>(3, 3) = 1;
        forinv = forinv.inv();
        rotation_matrix = forinv(cv::Rect(0, 0, 3, 3)); //从forinv中提取旋转矩阵R
        position_mat = forinv(cv::Rect(3, 0, 1, 3));    //从forinv中提取平移向量T

        base2gripper_R[i] = rotation_matrix.clone();
        base2gripper_T[i] = position_mat.clone();
        //cout<<"rotation matrix:\n"<<gripper2base_R[i]<<"        "<<gripper2base_T[i]<<endl;
    }

    Mat base2camera_R = Mat(3, 3, CV_64FC1, Scalar::all(0));
    Mat base2camera_T = Mat(3, 1, CV_64FC1, Scalar::all(0));
    Mat gripper2world_R = Mat(3, 3, CV_64FC1, Scalar::all(0));
    Mat gripper2world_T = Mat(3, 1, CV_64FC1, Scalar::all(0)); /* 创建矩阵用于保存结果 */

    calibrateRobotWorldHandEye(rotation_mats, tvecsMats,
                               base2gripper_R, base2gripper_T,
                               base2camera_R, base2camera_T,
                               gripper2world_R, gripper2world_T,
                               CALIB_ROBOT_WORLD_HAND_EYE_SHAH);

    // cout<<base2camera_R<<endl;
    // cout<<base2camera_T<<endl;
    Mat final_mat = Mat(4, 4, CV_64FC1, Scalar::all(0));
    // final_mat(Rect(0, 0, 3, 3)) = base2camera_R;
    // final_mat(Rect(3, 0, 1, 3)) = base2camera_T;
    for (int r = 0; r < 3; r++)
    {
        final_mat.at<double>(r, 3) = base2camera_T.at<double>(r);
        for (int c = 0; c < 3; c++)
        {
            final_mat.at<double>(r, c) = base2camera_R.at<double>(r, c);
        }
    }

    final_mat.at<double>(3, 3) = 1;
    Mat final_mat1 = Mat(4, 4, CV_64FC1, Scalar::all(0));
    final_mat1 = final_mat.inv();

    saveCalibrationFile(final_mat1,cameraMatrix,distCoeffs,image_size);
}

void HEcalibrator::saveCalibrationFile(cv::Mat in_extrinsic, cv::Mat in_intrinsic, cv::Mat in_dist_coeff, cv::Size in_size)
{
    std::string path_filename_str;
    const char *homedir;

    time_t rawtime;
    struct tm * timeinfo;
    char buffer[80];

    time (&rawtime);
    timeinfo = localtime(&rawtime);

    strftime(buffer,sizeof(buffer),"%Y%m%d_%H%M%S",timeinfo);
    std::string datetime_str(buffer);

    if ((homedir = getenv("HOME")) == NULL) {
        homedir = getpwuid(getuid())->pw_dir;
    }

    path_filename_str = std::string(homedir) + "/"+datetime_str+"_hand_eye_calibration.yaml";

    cv::FileStorage fs(path_filename_str.c_str(), cv::FileStorage::WRITE);
    if(!fs.isOpened()){
        fprintf(stderr, "%s : cannot open file\n", path_filename_str.c_str());
        exit(EXIT_FAILURE);
    }

    fs << "CameraExtrinsicMat" << in_extrinsic;
    fs << "CameraMat" << in_intrinsic;
    fs << "DistCoeff" << in_dist_coeff;
    fs << "ImageSize" << in_size;
    fs << "DistModel" << "plumb_bob";

    ROS_INFO("Wrote Calibration file in: %s", path_filename_str.c_str());
}

void HEcalibrator::initRosIO()
{
    ros::NodeHandle private_node_handle("~"); //to receive args

    private_node_handle.getParam("data_path", data_path);
    private_node_handle.getParam("chessboard_width", chessboard_width);
    private_node_handle.getParam("chessboard_height", chessboard_height);
    private_node_handle.getParam("square_dimension", square_dimension);
}


void HEcalibrator::Run()
{
    initRosIO();
    calibrateSensors();
}


HEcalibrator::HEcalibrator()
{
}


HEcalibrator::~HEcalibrator()
{
}